#include <STATE_MECHINE_module/STATE.hpp>

#ifdef DBG_TAG
#undef DBG_TAG
#endif
#ifdef DBG_LVL
#undef DBG_LVL
#endif

#define DBG_TAG "state"
#define DBG_LVL DBG_LOG

extern "C"
{
#include <rtdbg.h>
}

STATE::STATE(std::string state_name)
{
    this->state_name = state_name;
    system_status = state_status_none;

    data_lock = rt_mutex_create ((state_name + " state").c_str(), RT_IPC_FLAG_PRIO);
    if (!data_lock)
    {
        LOG_E("error:state lock <%s> create fail", state_name.c_str());
        goto end;
    }



    LOG_I("state <%s> create ok", state_name.c_str());
    init_flag = state_status_ok;
    return;
    end:
    LOG_E("error:state <%s> create fail", state_name.c_str());
    init_flag = state_status_fail;
    return;
}

STATE::~STATE()
{
    if(data_lock)rt_mutex_delete(data_lock);
}

state_status STATE::STATE_get_system_status()
{
    state_status temp;
    rt_mutex_take(data_lock, RT_WAITING_FOREVER);
    temp = system_status;
    rt_mutex_release(data_lock);
    return temp;
}

state_status STATE::STATE_set_system_status(state_status set_value)
{
    if(init_flag != state_status_ok)
    {
        LOG_E("error:state <%s> not init", state_name.c_str());
        goto end;
    }

    rt_mutex_take(data_lock, RT_WAITING_FOREVER);
    system_status = set_value;
    rt_mutex_release(data_lock);

    return state_status_ok;
    end:
    return state_status_fail;

}

state_status STATE::STATE_read_init_status()
{
    return init_flag;
}
